#include "WProgram.h"
#include "ServoPWMF.h"
#include <inttypes.h>
#include <avr/pgmspace.h>

//Use pin 3,9,10,11 for fingers. Use pin 5 or 6 for thumb
ServoPWMF::ServoPWMF() 
{
    //setup pins
    t=5;
    f1=11;
    f2=10;
    f3=9;
    f4=3;
}

//0<angleArg<180
void ServoPWMF::writeT(uint8_t angleArg)
{
    analogWrite( t , (180-angleArg)/180.0 * 27 + 5 );
    t_angle = angleArg;
}

void ServoPWMF::writeF1(uint8_t angleArg)
{
    analogWrite( f1 , angleArg/180.0 * 54 +12);
    f1_angle = angleArg;
}

void ServoPWMF::writeF2(uint8_t angleArg)
{
    analogWrite( f2 , (180-angleArg)/180.0 * 54 +12);
    f2_angle = angleArg;
}

void ServoPWMF::writeF3(uint8_t angleArg)
{
    analogWrite( f3 , angleArg/180.0 * 54 +12);
    f3_angle = angleArg;
}

void ServoPWMF::writeF4(uint8_t angleArg)
{
    analogWrite( f4 , (180-angleArg)/180.0 * 54 +12);
    f4_angle = angleArg;
}

void ServoPWMF::writeAll(uint8_t angleArg[])
{
    	this->writeT(angleArg[0]);
	this->writeF1(angleArg[1]);
	this->writeF2(angleArg[2]);
	this->writeF3(angleArg[3]);
	this->writeF4(angleArg[4]);

}

// incrementing angle ////////////////////////////
void ServoPWMF::incrementT(uint8_t maxAngleArg, int increment)
{
    int tempAngle = t_angle+increment;
    if( tempAngle > maxAngleArg && increment >0)
	tempAngle = maxAngleArg;
    else if( tempAngle < maxAngleArg && increment <0)
	tempAngle = maxAngleArg;
    writeT(tempAngle);
}

// incrementing angle ////////////////////////////
void ServoPWMF::incrementF1(uint8_t maxAngleArg, int increment)
{
    int tempAngle = f1_angle+increment;
    if( tempAngle > maxAngleArg && increment >0)
	tempAngle = maxAngleArg;
    else if( tempAngle < maxAngleArg && increment <0)
	tempAngle = maxAngleArg;
    writeF1(tempAngle);
}

// incrementing angle ////////////////////////////
void ServoPWMF::incrementF2(uint8_t maxAngleArg, int increment)
{
    int tempAngle = f2_angle+increment;
    if( tempAngle > maxAngleArg && increment >0)
	tempAngle = maxAngleArg;
    else if( tempAngle < maxAngleArg && increment <0)
	tempAngle = maxAngleArg;
    writeF2(tempAngle);
}

// incrementing angle ////////////////////////////
void ServoPWMF::incrementF3(uint8_t maxAngleArg, int increment)
{
    int tempAngle = f3_angle+increment;
    if( tempAngle > maxAngleArg && increment >0)
	tempAngle = maxAngleArg;
    else if( tempAngle < maxAngleArg && increment <0)
	tempAngle = maxAngleArg;
    writeF3(tempAngle);
}


// incrementing angle ////////////////////////////
void ServoPWMF::incrementF4(uint8_t maxAngleArg, int increment)
{
    int tempAngle = f4_angle+increment;
    if( tempAngle > maxAngleArg && increment >0)
	tempAngle = maxAngleArg;
    else if( tempAngle < maxAngleArg && increment <0)
	tempAngle = maxAngleArg;
    writeF4(tempAngle);
}

void ServoPWMF::setupTimer()
{
    TCCR0B = (TCCR0B & 0b11111000) | 0x05;
    TCCR1B = (TCCR1B & 0b11111000) | 0x04;
    TCCR2B = (TCCR2B & 0b11111000) | 0x06;      
}

void ServoPWMF::printAngles()
{
Serial.print("T:");
Serial.print((int)t_angle);

Serial.print("  f1:");
Serial.print((int)f1_angle);

Serial.print("  f2:");
Serial.print((int)f2_angle);

Serial.print("  f3:");
Serial.print((int)f3_angle);

Serial.print("  f4:");
Serial.println((int)f4_angle);

}

/*
Pins 5 and 6: controlled by Timer 0

Setting 	Divisor 	Frequency
0x01 	 	1 	 	62500
0x02  		8 	 	7812.5
0x03  		64 	 	976.5625
0x04 	 	256 	 	244.140625
0x05 	 	1024 	 	61.03515625    16.39ms

TCCR0B = TCCR0B & 0b11111000 | <setting>;


Pins 9 and 10: controlled by timer 1

Setting 	Divisor 	Frequency
0x01 	 	1 	 	31250
0x02 	 	8 	 	3906.25
0x03  		64 	 	488.28125
0x04  		256 	 	122.0703125   8.196ms
0x05 	 	1024 	 	30.517578125  32.769ms

TCCR1B = TCCR1B & 0b11111000 | <setting>;


Pins 11 and 3: controlled by timer 2

Setting 	Divisor 	Frequency
0x01 	 	1  		31250
0x02 	 	8 	 	3906.25
0x03  		32  		976.5625
0x04 	 	64 	 	488.28125
0x05 	 	128  		244.140625
0x06  		256  		122.0703125       8.196ms
0x07 	 	1024  		30.517578125      32.769ms
*/
